//VERSION 1.0 - Flown on Mini Bexus 26. Sept 2012 to ~10km alt
//added HIL

void vx_stabilize()
{
 
  
  Vector3f	gyro;
  imu.update();
  gyro = imu.get_gyro();
  
  /*Serial.print("Stab target Roll: ");
  Serial.print(vx_g_target_roll);*/
  
 calculate_speed_scaler();
 
  
 #if VX_SIM == VX_SIM_NONE
 vx_error_roll = ahrs.roll - vx_g_target_roll; //vx_g_target_roll [rad]
 #endif
 
 #if VX_SIM == VX_SIM_HIL
 vx_error_roll = vx_g_hil_roll - vx_g_target_roll; //vx_g_target_roll [rad]
 #endif
 
 
 vx_target_rollrate = -vx_error_roll * vx_g_stab_pidlistinuse->a_1;

if (vx_target_rollrate > vx_g_stab_pidlistinuse->a_5)
{
  vx_target_rollrate = vx_g_stab_pidlistinuse->a_5;
}

if (vx_target_rollrate < -vx_g_stab_pidlistinuse->a_5)
{
  vx_target_rollrate = -vx_g_stab_pidlistinuse->a_5;
}
 
 
 vx_error_rollrate = gyro.x - vx_target_rollrate;
 
 
 
 if (vx_error_rollrate >= 0)
  {
    if(vx_errorsum_rollrate < vx_g_stab_pidlistinuse->a_4)
    { 
      vx_errorsum_rollrate += vx_error_rollrate;
    }
  }
  else
  
  {
    if(vx_errorsum_rollrate > -vx_g_stab_pidlistinuse->a_4)
    { 
      vx_errorsum_rollrate += vx_error_rollrate;
    }
  } 
 

 
 vx_g_stabilizeout_roll = ((vx_g_stab_pidlistinuse->a_2) * vx_error_rollrate + (vx_g_stab_pidlistinuse->a_3) * vx_errorsum_rollrate) * vx_g_speed_scaler;
 
 /*
 Serial.print(ahrs.roll);
 Serial.print(" ");
 Serial.print(vx_g_target_roll);
 Serial.print(" ");
 Serial.print(vx_error_roll);
 Serial.print(" ");
 Serial.print(vx_g_stabilizeout_roll);
 Serial.println(" | ");
 */





    //nick is pitch in apm convention (we use ahrs library). we want to destinguish between nick and pitch because of helicopters. 
  //our convention: a plane has roll, nick, yaw, throttle
  //                a helicopter has roll, nick, yaw, pitch
  
#if VX_SIM == VX_SIM_NONE
vx_error_nick = ahrs.pitch - vx_g_target_nick;                               //vx_g_target_nick [rad]
#endif

#if VX_SIM == VX_SIM_HIL
vx_error_nick = vx_g_hil_nick - vx_g_target_nick;  
#endif


 vx_target_nickrate = -vx_error_nick * vx_g_stab_pidlistinuse->b_1;

if (vx_target_nickrate > vx_g_stab_pidlistinuse->b_5)
{
  vx_target_nickrate = vx_g_stab_pidlistinuse->b_5;
}

if (vx_target_nickrate < -vx_g_stab_pidlistinuse->b_5)
{
  vx_target_nickrate = -vx_g_stab_pidlistinuse->b_5;
}
 
 vx_error_nickrate = gyro.y - vx_target_nickrate;
 
 
 if (vx_error_nickrate >= 0)
  {
    if(vx_errorsum_nickrate < vx_g_stab_pidlistinuse->b_4)
    { 
      vx_errorsum_nickrate += vx_error_nickrate;
    }
    
    
  }
  else
  {
    if(vx_errorsum_nickrate > -vx_g_stab_pidlistinuse->b_4)
    { 
      vx_errorsum_nickrate += vx_error_nickrate;
    }
  } 
 
 vx_roll_to_elev_mix_value = abs(vx_g_stabilizeout_roll) * vx_g_stab_pidlistinuse->b_6;
 
 vx_g_stabilizeout_nick = ((vx_g_stab_pidlistinuse->b_2) * vx_error_nickrate + (vx_g_stab_pidlistinuse->b_3) * vx_errorsum_nickrate - vx_roll_to_elev_mix_value) * vx_g_speed_scaler;



  //Performance measurement
  if(vx_perf_a_count==1)
  {
    vx_perf_a_temptime = micros();
  }
  vx_perf_a_count++;
  if(vx_perf_a_count >= 50)
  {
    vx_g_perf_a_lastaveragecycletime =  (micros() - vx_perf_a_temptime) / 50;
    vx_perf_a_count = 1;
    //vx_g_pidlistinuse->roll_kita = (vx_g_pidlistinuse->roll_ki * vx_g_perf_a_lastaveragecycletime) / 1000000; // ki * Ta
    
    
    //Serial.println("");
    //Serial.print("cycle us: ");
    //Serial.print(vx_g_perf_a_lastaveragecycletime);
    
    
    
    /*
    Serial.print("gyro x, y ,z: ");
    Serial.print(gyro.x);// + "," + gyro.y + "," + gyro.z);
    Serial.println("");
    

    Serial.print(" || ");
    Serial.print(vx_error_roll);
    Serial.print(" || ");
    Serial.print(vx_errorsum_roll);
    Serial.print(" || ");
    Serial.print(vx_g_stabilizeout_roll);
    Serial.print(" || ");

    Serial.print(" min r: ");
    Serial.print(vx_g_rcin_diff_min_roll);
    Serial.print(" mid r: ");
    Serial.print(vx_g_rcin_middle_roll);  
    Serial.print(" max r: ");
    Serial.print(vx_g_rcin_diff_max_roll);
    Serial.print(" rel r: ");
    Serial.print(vx_g_rcin_roll());

    Serial.print(vx_g_rcin_nick());
    Serial.print(" || ");
    Serial.print("InCh0: ");  
    Serial.print(APM_RC.InputCh(0));
    Serial.print(" || ");
    Serial.print("InCh1: ");  
    Serial.print(APM_RC.InputCh(1));


    Serial.println("");

    //Serial.print(" ki * Ta: ");
    //Serial.print(vx_g_pidlistinuse->roll_kita);
    //Serial.println("");
    
    */
  }

  /*
  Serial.print(" || ");
   Serial.print("vx_error_roll: ");
   Serial.print(vx_error_roll);
   Serial.print(" || ");
   Serial.print("vx_g_stabilizeout_roll: ");
   Serial.print(vx_g_stabilizeout_roll);
   Serial.print(" || ");
   Serial.print("vx_error_nick: ");
   Serial.print(vx_error_nick);
   Serial.print(" || ");
   Serial.print("vx_g_stabilizeout_nick: ");
   Serial.print(vx_g_stabilizeout_nick);
   Serial.print(" || ");
   
   Serial.print("InCh0: ");  
   Serial.print(APM_RC.InputCh(0));
   Serial.print(" || ");
   Serial.print("InCh1: ");  
   Serial.print(APM_RC.InputCh(1));
   Serial.print(" || ");
   Serial.print("InCh7: ");  
   Serial.print(APM_RC.InputCh(7));
   Serial.print(" || ");
   
   
   Serial.print(" Stabilize: ");
   Serial.print(vx_g_flag_stabilize);
   
   Serial.print(" min r: ");
   Serial.print(vx_g_rcin_diff_min_roll);
   Serial.print(" mid r: ");
   Serial.print(vx_g_rcin_middle_roll);  
   Serial.print(" max r: ");
   Serial.print(vx_g_rcin_diff_max_roll);
   Serial.print(" rel r: ");
   Serial.print(vx_g_rcin_roll());
   
   Serial.print(" min n: ");
   Serial.print(vx_g_rcin_diff_min_nick);
   Serial.print(" mid n: ");
   Serial.print(vx_g_rcin_middle_nick);
   Serial.print(" max n: ");
   Serial.print(vx_g_rcin_diff_max_nick);
   Serial.print(" rel n: ");
   Serial.print(vx_g_rcin_nick());
   
   
   Serial.println("");
   
   */



};


void calculate_speed_scaler()
{
  vx_scaler_gpsspeed = 1.2 - (vx_g_current_ground_speed/10000);
  vx_g_speed_scaler = vx_scaler_gpsspeed * vx_g_scaler_tele;
  if (vx_g_speed_scaler > 1) vx_g_speed_scaler = 1.0;
  if (vx_g_speed_scaler < 0.1) vx_g_speed_scaler = 0.1;
}
  
  

